#include "CLineSegment3d.h"
#include "CLineSegment2d.h"
#include "CPlane.h"

fixed CLineSegment3d::ClosestPoint( const CVector3d& v0, const CVector3d& v1,
									const CVector3d& point, CVector3d& closestPoint )
{
	CVector3d v0v1 = v1 - v0;
	fixed u = ( point - v0 ) * v0v1; 

	if( u > 0 )
	{
		fixed d = v0v1 * v0v1;

		if( u < d )
		{
			u = CMathFixed::Div( u, d );
			closestPoint = v0 + u * v0v1;
		}
		else
		{
			u = CMathFixed::One;
			closestPoint = v1;
		}
	}
	else
	{
		u = 0;
		closestPoint = v0;
	}

	return u;
}


CVector3d& CLineSegment3d::ShortestVectorToPoint( const CVector3d& v0, const CVector3d& v1,
												  const CVector3d& point, 
												  CVector3d& v, fixed& u )
{
	u = ClosestPoint( v0, v1, point, v );
	v = point - v;

	return v;
}



void CLineSegment3d::ShortestVectorToSegment( const CVector3d& seg0V0, const CVector3d& seg0V1,
					 								const CVector3d& seg1V0, const CVector3d& seg1V1,
													CVector3d& c1, CVector3d& c2, fixed* u0, fixed *u1 )
{
	fixed EPSILON = CMathFixed_FloatToFixed( 0.001f );
	
	CVector3d   d1 = seg0V1 - seg0V0;
	CVector3d   d2 = seg1V1 - seg1V0;
	CVector3d   r = seg0V0 - seg1V0;

	fixed a = d1 * d1;
	fixed e = d2 * d2;
	fixed f = d2 * r;

	fixed &s = *u0;
	fixed &t = *u1;

	// check if both segments degenerate into points

	if ( a <= EPSILON && e <= EPSILON )
	{
		c1 = seg0V0;
		c2 = seg1V0;
		s = t = 0;
		return;
	}

	if ( a <= EPSILON )
	{
		// first segment degenrates into a point
		s = 0;
		t = CMathFixed::Div( f, e );
		t = CMathFixed::Clamp( t, 0, CMathFixed::One );
	}
	else
	{
		fixed c = d1 * r;
		if ( e <= EPSILON )
		{
			t = 0;
			s = CMathFixed::Clamp( CMathFixed::Div( -c, a ), 0, CMathFixed::One );
		}
		else
		{
			// the general non-degenrate case
			fixed b = d1 * d2;
			fixed denom = CMathFixed::Mul( a, e ) - CMathFixed::Mul( b, b );

			// if segments are not parallel, compute closest point on L1 to L2 and
			// clamp to segment s1, else pick arbitrary s

			if ( CMathFixed::Abs( denom ) > EPSILON )
			{
				s = CMathFixed::Clamp( CMathFixed::Div( CMathFixed::Mul( b,f ) - CMathFixed::Mul( c, e ), denom ), 0, CMathFixed::One );
			}
			else
				s = 0;

			fixed tNom = CMathFixed::Mul( b, s ) + f;

			if ( tNom < 0 )
			{
				t = 0;
				s = CMathFixed::Clamp( CMathFixed::Div( -c, a ), 0, CMathFixed::One );
			}
			else if ( tNom > e )
			{
				t = 1;
				s = CMathFixed::Clamp( CMathFixed::Div( b - c, a ), 0, CMathFixed::One );
			}
			else
				t = CMathFixed::Div( tNom, e );
		}
	}

	c1 = seg0V0 + d1 * s;
	c2 = seg1V0 + d2 * t;

}


boolean	CLineSegment3d::Intersects( const CVector3d& seg0V0, const CVector3d& seg0V1,
									const CVector3d& seg1V0, const CVector3d& seg1V1,
									fixed* u0, fixed *u1 )
{
	fixed EPSILON = CMathFixed_FloatToFixed( 0.001f );
	
	boolean intersects = FALSE;

	CPlane plane( seg0V0, seg0V1, seg1V0 );

	fixed distance = plane.GetSignedDistanceOfPoint( seg1V1 );

	if ( distance < EPSILON ) // the points are coplanar ... probably better to have an IsCoplanar method
	{
		// http://www.jtaylor1142001.net/calcjat/Solutions/VLines/VLIntersect.htm
		
		// now we go on to find u0 and u1 by projecting the 3d vectors onto one of the XY, YZ or XZ planes (by getting rid of the other component)
		// this follows from the fact that if two coplanar lines intersect, their projections will also intersect

		CLineSegment2d segA( seg0V0.m_i, seg0V0.m_j, seg0V1.m_i, seg0V1.m_j );
		CLineSegment2d segB( seg1V0.m_i, seg1V0.m_j, seg1V1.m_i, seg1V1.m_j );

		CPlane::AxisAlignedPlane aaplane = plane.GetClosestAxisAlignedPlane();

		if ( aaplane == CPlane::IJ )
		{
			segA.Set( CVector2d( seg0V0.m_i, seg0V0.m_j ), CVector2d( seg0V1.m_i, seg0V1.m_j ) );
			segB.Set( CVector2d( seg1V0.m_i, seg1V0.m_j ), CVector2d( seg1V1.m_i, seg1V1.m_j ) );
		}
		else if ( aaplane == CPlane::JK )
		{
			segA.Set( CVector2d( seg0V0.m_j, seg0V0.m_k ), CVector2d( seg0V1.m_j, seg0V1.m_k ) );
			segB.Set( CVector2d( seg1V0.m_j, seg1V0.m_k ), CVector2d( seg1V1.m_j, seg1V1.m_k ) );
		}
		else
		{
			segA.Set( CVector2d( seg0V0.m_k, seg0V0.m_i ), CVector2d( seg0V1.m_k, seg0V1.m_i ) );
			segB.Set( CVector2d( seg1V0.m_k, seg1V0.m_i ), CVector2d( seg1V1.m_k, seg1V1.m_i ) );
		}
		
		intersects = segA.Intersects( segB, u0, u1 );

		// we can verify that the 3rd component too satisfies this equation, but we have already done the coplanar test earlier
	}
	
	return intersects;
}
